#include "V4L2Controller.h"

V4L2Controller::V4L2Controller():
  m_width(0),m_height(0),m_fps(0),
  m_format(V4L2_STD_UNKNOWN),
  m_vstd(V4L2_STD_NTSC),
  m_fd(-1),m_framesize(0),
  m_tmpbytesused(0),
  m_tmpbuffer(NULL),
  m_framebuffer(NULL),
  m_streamingState(STREAMING_OFF)
{
    memset(m_videodevice,0,sizeof(m_videodevice));
    memset(&m_tmptimestamp,0,sizeof(m_tmptimestamp));
    memset(&m_frametimestamp,0,sizeof(m_frametimestamp));
    
}
V4L2Controller::~V4L2Controller()
{
    if(NULL != m_tmpbuffer)
        free(m_tmpbuffer);
    if(NULL != m_framebuffer)
        free(m_framebuffer);
}

int V4L2Controller::XIOCtl(int IOCTL_X, void* arg)
{
    int ret = 0;
    int tries = IOCTL_RETRY;
    do {
        ret = IOCTL_VIDEO(m_fd, IOCTL_X, arg);
    } while(ret && tries-- &&
            ((errno == EINTR) || (errno == EAGAIN) || (errno == ETIMEDOUT)));

    if(ret && (tries <= 0)) 
        fprintf(stderr, "ioctl (%i) retried %i times - giving up: %s)\n", IOCTL_X, IOCTL_RETRY, strerror(errno));

    return (ret);
}

int V4L2Controller::Initialize(const char* device,
                   int width,
                   int height,
                   int fps,
                   int format,
                   v4l2_std_id vstd)
{
    int i = 0;
    int ret = 0;
    snprintf(m_videodevice,(16-1),"%s",device);
    m_width = width;
    m_height = height;
    m_fps = fps;
    m_format = format;
    m_vstd = vstd;

    if((m_fd = OPEN_VIDEO(m_videodevice,O_RDWR)) == -1)
    {
        perror("ERROR opening V4L interface");
        return -1;
    }
    // Check Capability
    struct v4l2_capability cap;
    memset(&cap,0,sizeof(struct v4l2_capability));
    ret = XIOCtl(VIDIOC_QUERYCAP,&cap);
    if(ret < 0)
    {
        fprintf(stderr, "Error opening device %s: unable to query device.\n", m_videodevice);
        return -1;
    }

    if((cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) == 0)
    {
        fprintf(stderr, "Error opening device %s: video capture not supported.\n",
                m_videodevice);
        return -1;
    }

    if(!(cap.capabilities & V4L2_CAP_STREAMING)) {
        fprintf(stderr, "%s does not support streaming i/o\n", m_videodevice);
        return -1;
    }
    /*if (m_vstd != V4L2_STD_UNKNOWN) {
        if (ioctl(m_fd, VIDIOC_S_STD, &m_vstd) == -1) {
            fprintf(stderr, "Can't set video standard: %s\n",strerror(errno));
            return -1;
        }
    }*/
    // Set Format
    struct v4l2_format fmt;
    memset(&fmt,0,sizeof(struct v4l2_format));
    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    fmt.fmt.pix.width = m_width;
    fmt.fmt.pix.height = m_height;
    fmt.fmt.pix.pixelformat = m_format;
    fmt.fmt.pix.field = V4L2_FIELD_ANY;
    ret = XIOCtl(VIDIOC_S_FMT,&fmt);
    if(ret < 0)
    {
        fprintf(stderr, "Unable to set format: %d res: %dx%d\n", m_format, m_width, m_height);
    }
    // Set framerate
    if(m_fps > 0)
    {
        struct v4l2_streamparm* setfps;
        setfps = (struct v4l2_streamparm *)calloc(1,sizeof(struct v4l2_streamparm));
        memset(setfps,0,sizeof(struct v4l2_streamparm));
        setfps->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

        ret = XIOCtl(VIDIOC_G_PARM, setfps);
        if (ret == 0) 
        {
            if (setfps->parm.capture.capability & V4L2_CAP_TIMEPERFRAME) 
            {
                memset(setfps, 0, sizeof(struct v4l2_streamparm));
                setfps->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
                setfps->parm.capture.timeperframe.numerator = 1;
                setfps->parm.capture.timeperframe.denominator = m_fps==-1?255:m_fps; // if no default fps set set it to maximum

                ret = XIOCtl(VIDIOC_S_PARM, setfps);
                if (ret) 
                {
                    perror("Unable to set the FPS\n");
                } 
                
            } 
            else 
            {
                perror("Setting FPS on the capture device is not supported\n");
            }
        }
        else 
        {
            perror("Unable to query that the FPS change is supported\n");
        }
    }
    // request buffers
    struct v4l2_requestbuffers rb;
    memset(&rb,0,sizeof(struct v4l2_requestbuffers));
    rb.count = NB_BUFFER;
    rb.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    rb.memory = V4L2_MEMORY_MMAP;
    ret = XIOCtl(VIDIOC_REQBUFS,&rb);
    if(ret < 0)
    {
        perror("Unable to allocate buffers");
        return -1;
    }

    // map the buffers
    struct v4l2_buffer buf;
    for(i = 0;i<NB_BUFFER;i++)
    {
        memset(&buf,0,sizeof(struct v4l2_buffer));
        buf.index = i;
        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;
        ret = XIOCtl(VIDIOC_QUERYBUF,&buf);
        if(ret < 0)
        {
            perror("Unable to query buffer");
            return -1;
        }
        fprintf(stderr,"length: %u offset: %u\n",buf.length,buf.m.offset);
        m_mem[i] = mmap(0,buf.length,PROT_READ|PROT_WRITE,MAP_SHARED,
          m_fd,buf.m.offset);
        if(m_mem[i] == MAP_FAILED)
        {
            perror("Unable to map buffer");
            return -1;
        }
    }

    // Queue the buffers
    for(i = 0; i < NB_BUFFER; i++)
    {
        memset(&buf,0,sizeof(struct v4l2_buffer));
        buf.index = i;
        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;
        ret = XIOCtl(VIDIOC_QBUF,&buf);
        if(ret < 0)
        {
            perror("Unable to queue buffer");
            return -1;
        }
    }

     /* alloc a temp buffer to reconstruct the pict */
    m_framesize = (m_width * m_height << 1);
    switch(m_format) 
    {
    case V4L2_PIX_FMT_MJPEG: // in JPG mode the frame size is varies at every frame, so we allocate a bit bigger buffer
        m_tmpbuffer = (unsigned char *) calloc(1, (size_t) m_framesize);
        if(!m_tmpbuffer)
            return -1;
        m_framebuffer =
            (unsigned char *) calloc(1, (size_t) m_width * (m_height + 8) * 2);
        break;
    case V4L2_PIX_FMT_RGB565: // buffer allocation for non varies on frame size formats
    case V4L2_PIX_FMT_YUYV:
    case V4L2_PIX_FMT_UYVY:
        m_framebuffer =
            (unsigned char *) calloc(1, (size_t) m_framesize);
        break;
    default:
        fprintf(stderr, " should never arrive exit fatal !!\n");
        return -1;
        break;

    }

    if(!m_framebuffer)
        return -1;
    return 0; 
}
int V4L2Controller::GrabUVC()
{
    #define HEADERFRAME1 0xaf
    int ret;

    if(m_streamingState == STREAMING_OFF)
    {
        if(EnableVideo())
        {
            return -1;
        }
    }
    struct v4l2_buffer buf;
    memset(&buf,0,sizeof(struct v4l2_buffer));
    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory = V4L2_MEMORY_MMAP;

    ret = XIOCtl(VIDIOC_DQBUF,&buf);
    if(ret < 0)
    {
        perror("Unable to dequeue buffer");
        return -1;
    }

    switch(m_format)
    {
        case V4L2_PIX_FMT_MJPEG:
            if(buf.bytesused <= HEADERFRAME1)
            {
                fprintf(stderr,"Ignoring empty buffer ...\n");
                return 0;
            }
            memcpy(m_tmpbuffer,m_mem[buf.index],buf.bytesused);
            m_tmpbytesused = buf.bytesused;
            m_tmptimestamp = buf.timestamp;
            break;
        case V4L2_PIX_FMT_RGB565:
        case V4L2_PIX_FMT_YUYV:
        case V4L2_PIX_FMT_UYVY:
            if(buf.bytesused > m_framesize)
              memcpy(m_framebuffer,m_mem[buf.index],(size_t)m_framesize);
            else
              memcpy(m_framebuffer,m_mem[buf.index],(size_t)buf.bytesused);
            m_frametimestamp = buf.timestamp;
            break;
        default:
          return -1;
          break;
    }

    ret = XIOCtl(VIDIOC_QBUF,&buf);
    if(ret < 0)
    {
        perror("Unable to requeeu buffer");
        return -1;
    }
    return 0;
}
int V4L2Controller::CloseUVC()
{

}
int V4L2Controller::EnableVideo()
{
    int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    int ret;

    ret = XIOCtl(VIDIOC_STREAMON, &type);
    if(ret < 0) {
        perror("Unable to start capture");
        return ret;
    }
    m_streamingState = STREAMING_ON;
    return 0;
}
int V4L2Controller::DisableVideo(streaming_state disabledState)
{
    int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    int ret;
    //DBG("Stopping capture\n");
    ret = XIOCtl(VIDIOC_STREAMOFF, &type);
    if(ret != 0) {
        perror("Unable to stop capture");
        return ret;
    }
    //DBG("Stopping capture done\n");
    m_streamingState = disabledState;
    return 0;
}

